#include "bsp_pid_ctrl.h"

/*  @brief  PID控制
    @param  *obj
            PID对象结构体
    @param  set_val
            设定温度
    @param  now_Val
            此刻温度
    @return 无
    @property 无
*/
float bsp_PID_Control(PID_t *obj, float set_val, float now_Val)
{
    float coeP = obj->kp;
    float coeI = obj->ki;
    float coeD = obj->kd;

    obj->Set_Value = set_val;                    // 传入设定值
    obj->Now_Value = now_Val;                    // 传入实际值
    float err_val = set_val - now_Val;           // 误差（设定值和当前实际值误差）
    float err_change = err_val - obj->Pre_Error; // 误差变化量（当前误差-上一次误差）

    // 输出值= Kp * 当前误差 + Ki * 累计误差 + Kd * 误差变化量
    obj->PidOut = coeP * err_val + coeI * (obj->Total_Error) + coeD * err_change;

    obj->Total_Error += err_val; // 误差总和(累计偏差值)
    obj->Pre_Error = err_val;

    return (obj->PidOut);
}
